#include "Calibration.h"
#include "util.h"
#include <algorithm>
#include <iostream>
#include <fstream>

Calibration::Calibration ()
{
	calib_init_succ = false;
	wise = true;
}

Calibration::~Calibration ()
{
}

void Calibration::cal_R_h (latitude lat1, latitude lat2, double* R, double* h)
{
  //quite complicated
  double k = cos(lat1.lt)*lat2.dist/(cos(lat2.lt)*lat1.dist);
  double Rh = (k*sin(lat2.lt) - sin(lat1.lt))/(1-k);
  *R = (sin(lat1.lt)*Rh+1)*lat1.dist/(Rh+1)/cos(lat1.lt);
  *h = *R/Rh;

}

double Calibration::cal_lat_ang(double dist, double R, double h)
{
	double A = R*(R+h);
	double B = R*dist;
	double angle = acos(h*dist/sqrt(A*A+B*B)) - acos(A/sqrt(A*A+B*B));
	return angle;
}


double Calibration::cal_longitude(vector2dd coor,int *inx,double *k)
{
	int i,n;
	double angle = angle_0_360(dir_angle(coor.X,coor.Y)-dangle);
	double next_lg,this_lg,ret_lg;
	n = lglt.size();
	for(i=0;i<n-1;i++){
		
		next_lg = lglt[i+1].lg;
		this_lg = lglt[i].lg;
		if( wise && this_lg > next_lg){
			next_lg += M_PI*2;
		}else if(!wise && this_lg<next_lg){
			this_lg += M_PI*2;
		}
		if( (angle-lglt[i].angle) * (angle-lglt[i+1].angle)<0 ){
			*inx = i;
			*k = (angle-lglt[i].angle)/(lglt[i+1].angle - lglt[i].angle);
			ret_lg = angle_0_360(this_lg + (next_lg-this_lg) * (*k));
			return ret_lg;
		}
	}
	//the program is not supposed to reach here
	return -1;
}

double Calibration::cal_latitude(double dist,int inx,double k)
{
	int i,n = lts.size();
	double R,h;
	if(lts.size()<2)
		return -1; //error
	if(dist<lts[0].dist){
		cal_R_h(lts[0],lts[1],&R,&h);
		return fabs(cal_lat_ang(dist,R,h));
	}
	for(i=0;i<n-1;i++){
		if( (dist-lts[i].dist)*(dist-lts[i+1].dist) ){
			cal_R_h(lts[i],lts[i+1],&R,&h);
			return fabs(cal_lat_ang(dist,R,h));
		}
	}
	cal_R_h(lts[n-2],lts[n-1],&R,&h);
	return fabs(cal_lat_ang(dist,R,h));

}

vector2dd Calibration::transform_coor(vector2dd cam)
{
	if(cam.X < 0 && cam.Y < 0){
		return vector2dd(-1,-1);
	}
	vector2dd r_cam;
	vector2dd res;
	double k;
	int inx;

	if(!calib_init_succ){
		return r_cam;
	}else{
		r_cam = cam - cam_center;
	}

	r_cam.Y = -r_cam.Y;
	res.X = cal_longitude(r_cam,&inx,&k);
	res.Y = cal_latitude(r_cam.getLength(),inx,k);
	return res;

}

bool lg_comp(const longitude& l1, const longitude& l2)
{
	return l1.angle < l2.angle;
}

void Calibration::cal_lts(void)
{
	int i,j,lt_num,lg_num;
	latitude lt;
	lg_num = lglt.size();		//num of longitudes
	lt_num = lglt[0].lat.size(); //num of latitudes
	for(i=0;i<lt_num;i++){
		lt.lt = lglt[0].lat[i].lt;
		lt.dist = 0;
		for(j=0;j<lg_num;j++){
			lt.dist += lglt[j].lat[i].dist;
		}
		lt.dist /=lg_num;
		lts.push_back(lt);
	}
}

bool Calibration::init_from_file(const char *filename)
{
	int i,j,lg_num,lt_num;
	int wise_num=0,c_wise_num=0;
	longitude lg;
	latitude lt;
	ifstream ist(filename);  
	if(ist==NULL){  
		cerr<<"read file failed"<<endl;
		calib_init_succ = false;
		return false;
	}
	lglt.clear();

	ist>>cam_center.X;	ist>>cam_center.Y;

	ist>>lg_num;	ist>>lt_num;
	for(i=0;i<lg_num;i++){
		ist>>lg.angle;
		ist>>lg.lg;
		lg.lat.clear();
		for(j=0;j<lt_num;j++){
			ist>>lt.dist;
			ist>>lt.lt;
			lg.lat.push_back(lt);
		}
		lglt.push_back(lg);
	}
	ist.close();

	sort(lglt.begin(),lglt.end(),lg_comp);
	lg = lglt[0];
	dangle = angle_0_360(lg.angle);
	for(i=0;i<lg_num;i++){
		lglt[i].angle = angle_0_360(lglt[i].angle - dangle);
	}
	lg = lglt[0];
	lg.angle = M_PI*2;
	lglt.push_back(lg);

	for(i=0;i<lg_num;i++){
		if(lglt[i].lg < lglt[i+1].lg){
			++wise_num;
		}else{
			++c_wise_num;
		}
	}
	if(wise_num > c_wise_num){
		wise = true;
	}else{
		wise = false;
	}

	cal_lts();

	calib_init_succ = true;
	return true;
	
}
